﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

/*
 * 曲线工具脚本：        
 * Bezier,
 * BSpine2,
 * BSpine3,
 * CatmulRom,
 * Hermite
 */
public static class CurveUtily
{
    #region Bezier
    public static Vector3 Bezier3(
    float t, Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
    {
        t = Mathf.Clamp01(t);
        float d = 1f - t;
        return d * d * d * p0 + 3f * d * d * t * p1
            + 3f * d * t * t * p2 + t * t * t * p3;
    }

    public static Vector3 BezierTangent3(
    float t, Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
    {
        t = Mathf.Clamp01(t);
        return (-3f * p0 + 9f * p1 - 9f * p2 + 3f * p3) * (t * t)
            + (6f * p0 - 12f * p1 + 6f * p2) * t
            - 3f * p0 + 3f * p1;
    }
    public static float Bezier1(float t, float p0, float p1, float p2, float p3)
    {
        t = Mathf.Clamp01(t);
        float d = 1f - t;
        return d * d * d * p0 + 3f * d * d * t * p1
            + 3f * d * t * t * p2 + t * t * t * p3;
    }

    public static float BezierTangent1(
        float t, float p0, float p1, float p2, float p3)
    {
        t = Mathf.Clamp01(t);
        return (-3f * p0 + 9f * p1 - 9f * p2 + 3f * p3) * t * t
            + (6f * p0 - 12f * p1 + 6f * p2) * t
            - 3f * p0 + 3f * p1;
    }

    /// <summary>
    /// 求阶乘:n!
    /// </summary>
    /// <param name="n"></param>
    /// <returns></returns>
    public static int Factorial(this int n)
    {
        if (n == 0)
        {
            return 1;
        }
        return n * (n - 1).Factorial();
    }
    /// <summary>
    /// 求组合
    /// </summary>
    public static int Combination(this int n, int m)
    {
        return n.Factorial() / (m.Factorial() * (n - m).Factorial());
    }

    /// <summary>
    /// 创建Bezier曲线
    /// </summary>
    public static void CreateBezierCurve(Vector3[] points, int amountRadio, ref List<Vector3> wayPoints)
    {
        int SmoothAmount = (points.Length - 1) * amountRadio;
        for (int i = 0; i <= SmoothAmount; i++)
        {
            float t = i * 1f / SmoothAmount;
            wayPoints.Add(GetBezierPoint(points,t));
        }
    }

    /// <summary>
    /// 贝塞尔曲线点
    /// </summary>
    public static Vector3 GetBezierPoint(Vector3[] p, float t)
    {
        int n = p.Length - 1;
        float u = 1 - t;

        Vector3 result = Vector3.zero;

        for (int i = 0; i < p.Length; i++)
        {
            result += p[i] * Mathf.Pow(u, n - i) * Mathf.Pow(t, i) * Combination(n, i);
        }

        return result;
    }

    #endregion


    #region BSpine
    /// <summary>
    /// 二次B样条
    /// </summary>
    /// <param name="points">控制节点</param>
    /// <param name="amountRadio">曲线采样精度</param>
    /// <param name="close">曲线闭合</param>
    /// <param name="wayPoints">曲线数组</param>
    public static void CreateB2SpineCurve(Vector3[] points, int amountRadio, bool close, ref List<Vector3> wayPoints)
    {
        if (points == null || points.Length <= 1) { Debug.Log("points is empty!"); return; }

        wayPoints.Clear();

        Vector3[] vector3s = points;
        if (close)
        {
            int offset = 2;
            vector3s = new Vector3[points.Length + offset];
            Array.Copy(points, vector3s, points.Length);

            vector3s[vector3s.Length - 2] = points[0];
            vector3s[vector3s.Length - 1] = points[1];
        }

        int SmoothAmount = (points.Length - 1) * amountRadio;
        for (int i = 0; i <= SmoothAmount; i++)
        {
            float t = (float)i / SmoothAmount;
            Vector3 currPt = GetB2SpinePosition(vector3s, t);
            wayPoints.Add(currPt);
        }
    }
    public static Vector3 GetB2SpinePosition(Vector3[] pts, float t)
    {

        int numSections = pts.Length - 2;
        int currPt = Mathf.Min(Mathf.FloorToInt(t * (float)numSections), numSections-1); 
        float u = t * (float)numSections - (float)currPt;
        float u2 = u * u;

        Vector3 p0 = pts[currPt] * (u2 - 2 * u + 1) * 0.5f;
        Vector3 p1 = pts[currPt + 1] * (-2 * u2 + 2 * u + 1) * 0.5f;
        Vector3 p2 = pts[currPt + 2] * u2 * 0.5f;

        return p0 + p1 + p2;
    }
    /// <summary>
    /// 三次B样条
    /// </summary>
    /// <param name="points">控制节点</param>
    /// <param name="amountRadio">曲线采样精度</param>
    /// <param name="wayPoints">曲线数组</param>
    public static void CreateB3SpineCurve(Vector3[] points, int amountRadio, ref List<Vector3> wayPoints)
    {
        if (points == null || points.Length <= 1) { Debug.Log("points is empty!"); return; }

        wayPoints.Clear();

        int offset = 2;
        Vector3[] vector3s = new Vector3[points.Length + offset];
        Array.Copy(points,0, vector3s,1, points.Length);

        //计算第一个控制点和最后一个控制点位置
        vector3s[0] = vector3s[1] + (vector3s[1] - vector3s[2]);
        vector3s[vector3s.Length - 1] = vector3s[vector3s.Length - 2] + (vector3s[vector3s.Length - 2] - vector3s[vector3s.Length - 3]);

        int SmoothAmount = (points.Length - 1) * amountRadio;
        for (int i = 0; i <= SmoothAmount; i++)
        {
            float t = (float)i / SmoothAmount;
            Vector3 currPt = GetB3SpinePosition(vector3s, t);
            wayPoints.Add(currPt);
        }
    }
    public static Vector3 GetB3SpinePosition(Vector3[] pts, float t)
    {
        int numSections = pts.Length - 3;
        int currPt = Mathf.Min(Mathf.FloorToInt(t * (float)numSections), numSections - 1);
        float u = t * (float)numSections - (float)currPt;
        float u2 = u * u;
        float u3 = u2 * u;

        Vector3 p0 = pts[currPt] * (-1 * u3 + 3 * u2 - 3 * u + 1) / 6.0f;
        Vector3 p1 = pts[currPt + 1] * (3 * u3 - 6 * u2 + 0 * u + 4) / 6.0f;
        Vector3 p2 = pts[currPt + 2] * (-3 * u3 + 3 * u2 + 3 * u + 1) / 6.0f;
        Vector3 p3 = pts[currPt + 3] * (1 * u3 + 0 * u2 + 0 * u + 0) / 6.0f;

        return p0 + p1 + p2+p3;
    }

    #endregion

    #region CatmullRom

    /// <summary>
    /// CatmullRom曲线
    /// </summary>
    /// <param name="points">控制节点</param>
    /// <param name="amountRadio">曲线采样精度</param>
    /// <param name="close">曲线闭合</param>
    /// <param name="wayPoints">曲线数组</param>
    public static void CreateCatmullRomCurve(Vector3[] points, int amountRadio,bool close, ref List<Vector3> wayPoints)
    {
        if (points == null || points.Length <= 1) { Debug.Log("points is empty!"); return; }

        wayPoints.Clear();

        Vector3[] vector3s = PathControlPointGenerator(points, close);
        int SmoothAmount = (points.Length - 1) * amountRadio;
        for (int i = 0; i <= SmoothAmount; i++)
        {
            float t = (float)i / SmoothAmount;
            Vector3 currPt = GetCatmullRomPosition(vector3s, t);
            wayPoints.Add(currPt);
        }
    }
    

    public static Vector3 GetCatmullRomPosition(Vector3[] pts, float t)
    {
        //曲线每4点分一段，4点1段，5点2段，6点3段...
        int numSections = pts.Length - 3;
        int currPt = Mathf.Min(Mathf.FloorToInt(t * (float)numSections), numSections - 1); //t=0.5时分段
        float u = t * (float)numSections - (float)currPt;
        float u2 = u * u;
        float u3 = u2 * u;

        Vector3 a = pts[currPt];
        Vector3 b = pts[currPt + 1];
        Vector3 c = pts[currPt + 2];
        Vector3 d = pts[currPt + 3];

        return 0.5f * (
            (-a + 3f * b - 3f * c + d) * u3 +
            (2f * a - 5f * b + 4f * c - d) * u2 +
            (-a + c) * u +
            2f * b
        );
    }

    #endregion


    #region Hermite
    /// <summary>
    /// CatmullRom曲线
    /// </summary>
    /// <param name="points">控制节点</param>
    /// <param name="amountRadio">曲线采样精度</param>
    /// <param name="hermiteAlpha">逼近值(0-1),在Cat曲线和直线之间逼近。0为CatmullRom曲线，1为直线</param>
    /// <param name="close">曲线闭合</param>
    /// <param name="wayPoints">曲线数组</param>
    public static void CreateHermiteCurve(Vector3[] points, int amountRadio,float hermiteAlpha, bool close, ref List<Vector3> wayPoints)
    {
        if (points == null || points.Length <= 1) { Debug.Log("points is empty!"); return; }

        wayPoints.Clear();

        Vector3[] vector3s = PathControlPointGenerator(points, close);
        int SmoothAmount = (points.Length - 1) * amountRadio;
        for (int i = 0; i <= SmoothAmount; i++)
        {
            float t = (float)i / SmoothAmount;
            Vector3 currPt = GetHermitePosition(vector3s, hermiteAlpha, t);
            wayPoints.Add(currPt);
        }
    }

    public static Vector3 GetHermitePosition(Vector3[] pts, float hermiteAlpha, float t)
    {
        //曲线每4点分一段，4点1段，5点2段，6点3段...
        int numSections = pts.Length - 3;
        int currPt = Mathf.Min(Mathf.FloorToInt(t * (float)numSections), numSections - 1); //t=0.5时分段
        float u = t * (float)numSections - (float)currPt;
        float u2 = u * u;
        float u3 = u2 * u;

        Vector3 a = pts[currPt];
        Vector3 b = pts[currPt + 1];
        Vector3 c = pts[currPt + 2];
        Vector3 d = pts[currPt + 3];

        Vector3 p0 = a * -(u3 - (2 * u2) + u) * (1 - hermiteAlpha) * 0.5f;
        Vector3 p1 = b * (2 * u3 - 3 * u2 + 1 - (u3 - u2) * (1 - hermiteAlpha) * 0.5f);
        Vector3 p2 = c * (-2 * u3 + 3 * u2 + (u3 - 2 * u2 + u) * (1 - hermiteAlpha) * 0.5f);
        Vector3 p3 = d * (u3 - u2) * (1 - hermiteAlpha) * 0.5f;

        return p0 + p1 + p2 + p3;
    }

    #endregion

    private static Vector3[] PathControlPointGenerator(Vector3[] path, bool isClose)
    {
        Vector3[] suppliedPath;
        Vector3[] vector3s;

        suppliedPath = path;

        int offset = 2;
        vector3s = new Vector3[suppliedPath.Length + offset];
        Array.Copy(suppliedPath, 0, vector3s, 1, suppliedPath.Length);

        //计算第一个控制点和最后一个控制点位置
        vector3s[0] = vector3s[1] + (vector3s[1] - vector3s[2]);
        vector3s[vector3s.Length - 1] = vector3s[vector3s.Length - 2] + (vector3s[vector3s.Length - 2] - vector3s[vector3s.Length - 3]);
        if (isClose)
        {
            //首位点重合时，形成闭合的Catmull-Rom曲线
            vector3s[1] = vector3s[vector3s.Length - 2];

            Vector3[] tmpLoopSpline = new Vector3[vector3s.Length];
            Array.Copy(vector3s, tmpLoopSpline, vector3s.Length);
            tmpLoopSpline[0] = tmpLoopSpline[tmpLoopSpline.Length - 3];
            tmpLoopSpline[tmpLoopSpline.Length - 1] = tmpLoopSpline[2];
            vector3s = new Vector3[tmpLoopSpline.Length];
            Array.Copy(tmpLoopSpline, vector3s, tmpLoopSpline.Length);
        }

        return (vector3s);
    }


    public static void ComputeSmoothControlPoints(ref Vector4[] knot,ref Vector4[] ctrl1, ref Vector4[] ctrl2)
    {
        int numPoints = knot.Length;
        if(numPoints <= 2)
        {
            if (numPoints == 2)
            {
                ctrl1[0] = Vector4.Lerp(knot[0], knot[1], 0.33333f);
                ctrl2[0] = Vector4.Lerp(knot[0], knot[1], 0.66666f);
            }
            else if (numPoints == 1) ctrl1[0] = ctrl2[0] = knot[0];
            return;
        }
        var a = new float[numPoints];
        var b = new float[numPoints];
        var c = new float[numPoints];
        var r = new float[numPoints];
        for(int axis = 0;axis < 4; ++axis)
        {
            int n = numPoints - 1;
            a[0] = 0;
            b[0] = 2;
            c[0] = 1;
            r[0] = knot[0][axis] + 2 * knot[1][axis];

            for (int i = 1; i < n - 1; ++i)
            {
                a[i] = 1;
                b[i] = 4;
                c[i] = 1;
                r[i] = 4 * knot[i][axis] + 2 * knot[i + 1][axis];
            }

            a[n - 1] = 2;
            b[n - 1] = 7;
            c[n - 1] = 0;
            r[n - 1] = 8 * knot[n - 1][axis] + knot[n][axis];

            // Solve with Thomas algorithm
            for (int i = 1; i < n; ++i)
            {
                float m = a[i] / b[i - 1];
                b[i] = b[i] - m * c[i - 1];
                r[i] = r[i] - m * r[i - 1];
            }

            // Compute ctrl1
            ctrl1[n - 1][axis] = r[n - 1] / b[n - 1];
            for (int i = n - 2; i >= 0; --i)
                ctrl1[i][axis] = (r[i] - c[i] * ctrl1[i + 1][axis]) / b[i];

            // Compute ctrl2 from ctrl1
            for (int i = 0; i < n; i++)
                ctrl2[i][axis] = 2 * knot[i + 1][axis] - ctrl1[i + 1][axis];
            ctrl2[n - 1][axis] = 0.5f * (knot[n][axis] + ctrl1[n - 1][axis]);
        }

    }

    public static void ComputeSmoothControlPointsLooped(
    ref Vector4[] knot, ref Vector4[] ctrl1, ref Vector4[] ctrl2)
    {
        int numPoints = knot.Length;
        if (numPoints < 2)
        {
            if (numPoints == 1)
                ctrl1[0] = ctrl2[0] = knot[0];
            return;
        }

        int margin = Mathf.Min(4, numPoints - 1);
        Vector4[] knotLooped = new Vector4[numPoints + 2 * margin];
        Vector4[] ctrl1Looped = new Vector4[numPoints + 2 * margin];
        Vector4[] ctrl2Looped = new Vector4[numPoints + 2 * margin];
        for (int i = 0; i < margin; ++i)
        {
            knotLooped[i] = knot[numPoints - (margin - i)];
            knotLooped[numPoints + margin + i] = knot[i];
        }
        for (int i = 0; i < numPoints; ++i)
            knotLooped[i + margin] = knot[i];
        ComputeSmoothControlPoints(ref knotLooped, ref ctrl1Looped, ref ctrl2Looped);
        for (int i = 0; i < numPoints; ++i)
        {
            ctrl1[i] = ctrl1Looped[i + margin];
            ctrl2[i] = ctrl2Looped[i + margin];
        }
    }

}
